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ABSTRACT 

Nonlinearities  appear  “everywhere”  in  the  signal  and  data 
processing  chain  of  the  Global  Navigation  Satellite 
System  (GNSS).  At  the  “upper”  end  of  the  chain, 
ephemeris  data  modulated  onto  transmitted  signals  are 
predicted  from  satellite  orbits  whose  determination  is  a 
well-known  nonlinear  estimation  problem.  At  the  “lower” 
end,  within  a  GNSS  receiver,  the  satellite  signal  tracking, 
position-fixing,  and  even  integration  with  other  sensors, 
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such  as  an  inertial  navigation  system  (INS),  all  involve 
nonlinearity  issues  in  one  form  or  another.  Either  a  small 
signal  model  or  linearization  is  presently  used  to  deal  with 
nonlinearity.  The  former  includes  code  and  carrier 
tracking  loops  and  the  latter  includes  the  extended 
Kalman  filter  (EKF)  for  orbit  determination,  position 
solution,  and  GPS/INS  integration  among  others. 

In  this  paper,  we  present  two  emerging  nonlinear  filtering 
techniques,  namely,  the  unscented  Kalman  filter  (UKF) 
and  particle  filter  (PF),  and  study  their  use  in  GNSS 
applications  in  comparison  to  the  EKF.  The  UKF  is  also 
called  the  sigma-point  Kalman  filter  (SPKF)  and  the  PF 
has  many  variants  in  its  implementation.  In  the  EKF,  both 
the  state  dynamics  and  measurement  equations  are 
linearized  in  order  to  apply  the  Kalman  filter,  which  is 
only  valid  for  linear  Gaussian  systems.  Instead  of 
truncating  the  nonlinear  functions  to  the  first  order  as  in 
the  EKF,  the  UKF  and  PF  approximate  the  distribution  of 
the  state  deterministically  (sigma  points)  and  randomly 
(particles),  respectively,  with  a  finite  set  of  samples,  and 
then  propagate  these  points  or  particles  through  the  exact 
nonlinear  functions.  Because  the  nonlinear  functions  are 
used  without  approximation,  it  is  much  simpler  to 
implement  and  generates  better  results. 

After  formulating  these  nonlinear  filtering  algorithms,  this 
paper  will  illustrate  their  functionality  and  performance 
using  satellite  orbit  determination  as  an  example  via 
computer  simulation.  Furthermore,  we  will  discuss 
implementation  issues  and  analyze  the  use  of  these 
nonlinear  filtering  techniques  to  solve  other  nonlinear 
problems  in  GNSS  and  navigation  applications. 

INTRODUCTION 

Kalman  filtering  is  widely  used  in  both  GPS  Control 
Segment  and  User  Segment.  The  GPS  user  solution  is 
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calculated  by  a  Kalman  filter  embedded  in  the  GPS 
receiver,  which  makes  use  of  the  Space  Vehicle  (SV) 
orbit  and  time  scale  information  contained  in  the 
navigation  data  sets  generated  by  the  Operational  Control 
Segment  (OCS)  again  using  a  Kalman  filter. 

It  is  well  known  that  the  Kalman  filter  was  developed 
based  upon  linear  dynamic  and  measurement  models  with 
additive  white  Gaussian  noises.  Under  these  assumptions, 
the  state  and  its  propagation  remain  Gaussian  and  are 
completely  characterized  by  the  probability  density 
functions  in  terms  of  the  mathematical  expectation  (i.e., 
mean  value)  and  covariance  matrix.  Indeed,  the  Kalman 
filter  provides  the  recursive  equations  to  compute  the 
mean  value  (i.e.,  the  state  estimate)  and  the  estimation 
error  covariance  for  propagation  over  time  and  for 
updating  when  the  state  measurements  are  available. 

For  many  practical  applications,  however,  the  state 
dynamic  and/or  measurement  equations  are  not  linear.  In 
order  to  apply  the  Kalman  filter,  conventional  approaches 
would  linearize  the  nonlinear  dynamic  and  measurement 
equations  using  Taylor  series  expansion  (usually  to  the 
first  order  but  sometimes  to  the  second  order  terms)  either 
around  the  predicted  state  values  or  along  a  pre-calculated 
reference  trajectory,  resulting  in  the  so-called  extended 
Kalman  filter  (EKF)  [Maybeck,  1982],  This  EKF 
approach  has  been  used  successfully  in  many  applications 
but  its  implementation  requires  extensive  tuning, 
continuous  monitoring,  and  on-line  adjustment  if 
necessary  to  avoid  bias  and  instability. 

It  has  been  shown  that  it  is  much  easier  to  approximate  a 
Gaussian  distribution  than  it  is  to  approximate  arbitrary 
nonlinear  functions.  Based  on  this,  the  unscented  Kalman 
filter  (UKF)  has  recently  been  introduced  [Julier,  Uhlman 
and  Durrant- Whyte,  1995;  Julier,  2002;  Julier  and 
Uhlman,  2004],  which  produces  more  accurate  results 
than  the  EKF.  Instead  of  truncating  nonlinear  functions  to 
first  order  as  in  the  EKF,  the  UKF  approximates  the 
distribution  of  the  state  with  a  finite  set  of  samples  (i.e., 
deterministically  selected  from  the  Gaussian  distribution). 
These  samples,  called  sigma  points,  are  then  propagated 
through  the  true  nonlinear  functions.  The  mean  and 
covariance  of  the  distribution  after  propagation  and 
update  are  calculated  as  the  weighted  sum  of  the 
propagated  sigma  points  and  the  weighted  sum  of  their 
outer  products,  respectively.  Because  the  nonlinear 
functions  are  used  without  approximation  nor  derivatives, 
it  is  much  simpler  to  implement  and  generates  better 
results  (the  performance  is  superior  or  similar  to  a 
truncated  second  order  EKF  but  without  the  need  to 
calculate  Jacobians  or  Hessians  of  the  nonlinear 
functions).  However,  the  UKF  cannot  be  applied  to 
general  non-Gaussian  distributions. 

Sequential  Monte  Carlo  methods  or  particle  filters 
[Doucet,  de  Freitas,  and  Gordon,  2000;  Liu  2001]  allow 


for  a  complete  representation  of  the  posterior  distribution 
of  the  states  so  that  any  statistical  estimates  such  as 
means,  modes,  kurtosis  and  variances  can  be  computed, 
thus  providing  a  tool  to  deal  with  any  nonlinearities  or 
distributions.  The  basis  of  particle  filters  is  the  sequential 
importance  sampling  (SIS)  algorithm,  which  is  also 
known  as  bootstrap  filtering,  interacting  particle 
approximation,  and  survival  of  the  fittest  among  others. 
The  key  idea  is  to  represent  the  posterior  density  function 
of  the  state  given  measurements  by  a  set  of  random 
samples  with  associated  weights  and  to  compute  the 
estimates  based  on  the  samples  and  weights.  This  point 
mass  (hence  the  name  “particle”)  representation  of  a 
probability  density  generalizes  the  traditional  Kalman 
filtering  method  to  nonlinear,  non-Gaussian,  on-line 
estimation  [Arulampalam  et  al.,  2002]. 

The  crucial  step  in  the  design  of  a  particle  filter  is 
therefore  how  to  choose  the  importance  function  or 
proposal  distribution  that  can  approximate  the  desired 
unknown  posterior  distribution  reasonably  well.  The  most 
common  choice  is  to  sample  from  the  probabilistic  model 
of  the  state  evolution  (transition  prior)  but  it  may  fail  if 
measurements  appear  in  the  tail  of  the  prior  or  if  the 
likelihood  function  is  too  peaked  in  comparison  to  the 
prior.  To  overcome  this  problem,  the  EKF  or  UKF -based 
Gaussian  approximation  can  be  used  as  the  proposal 
distribution  [Haykin  2001].  The  latter  also  allows  the 
control  of  the  rate  at  which  the  tails  of  the  proposal 
importance  distribution  go  to  zero,  thus  generating 
proposal  distribution  with  larger  high  order  moments  and 
means  that  are  closer  to  the  true  mean  of  the  target 
distribution  [van  de  Merwe,  2004]. 

In  this  paper,  we  are  interested  in  these  emerging 
nonlinear  filtering  techniques  for  GNSS  related 
applications.  In  principle,  they  can  be  used  wherever  the 
EKF  is  used  today  in  GNSS  and  other  navigation 
applications.  This  includes  orbit  determination  to  be 
illustrated  in  this  paper,  UKF  or  PF-based  position  fixing 
with  pseudoranges  in  GPS  receiver,  carrier  phase  tracking 
with  PF  [Amblard,  Brassier,  and  Moisan,  2002],  integer 
ambiguity  resolution  with  PF  [Azimi-Sadjadi  and 
Krishnaprasad,  2001],  blind  equalization  and  estimation 
in  fading  channels  [Djuric  et  al.,  2003],  and  GPS/INS 
integration  with  PF  [Frykman,  2003].  The  EKF  used  in 
joint  multipath  estimation  and  PRN  code  acquisition  [litis, 
1990]  can  be  replaced  by  a  UKF  or  PF.  Other  applications 
in  navigation  include  quaternion-based  orientation 
tracking  [Kraft,  2003],  cellular  phone  measurements- 
based  positioning,  map  matching  to  digital  elevation 
profile,  and  collision  avoidance  for  cars  [Gustafsson  et  al., 
2003;  Pham,  Dahia,  and  Musso,  2003]. 

After  formulating  the  nonlinear  filtering  algorithms,  we 
will  discuss  their  implementation  issues  in  this  paper.  We 
will  present  computer  simulation  results  to  illustrate  the 
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functionality  and  performance  of  the  nonlinear  filters 
using  satellite  orbit  determination  as  an  example. 

NONLINEAR  FILTERING  WITH  EKF,  UKF  &  PF 

In  this  section,  some  equivalent  models  that  are  suitable 
for  nonlinear  filter  development  are  first  described  for 
nonlinear  dynamic  and  measurement  equations.  The 
UKF/SPKF  and  PF  are  then  presented.  For  simplicity,  the 
details  for  the  EKF  are  omitted,  which  can  be  found  in 
many  textbooks  such  as  [Maybeck,  1982]. 

Nonlinear  Dynamic  and  Measurement  Models 

A  nonlinear  dynamic  system  can  be  represented  by  a  state 
transition  model  with  at  least  three  forms,  namely,  the 
probability  density,  regression,  and  first-order 
linearization: 

p(x,\x,-i)  (la) 

Xi-i,  Vj,-i)  (lb) 

+  -v)  (1c) 

where  x,  e  Rnx  denotes  the  state  (including  the  hidden 
variables  and/or  parameters)  of  the  system  at  time  t,  u,  e 
Rnu  is  some  known  input,  vt  £  Rnv  is  a  noise  term  with 
covariance  matrix  Q,  f:  Rnx  x  Rnu  x  Rnv  — >  Rnx  is  the 
deterministic  nonlinear  state  transition  model,  and 
p  _  <n*,)  |  and  r,  -Mi  are  the  Jacobians  of  the 

1  t  8x,  \x,=x,  dv,  lv,=v 

state  model  linearized  around  the  reference  value  x  and 
v,  respectively. 

By  the  same  token,  the  nonlinear  measurement  model  can 
be  put  into  one  of  the  three  forms  as: 

P(y.t\Xj)  (2a) 

Yi  =  h(uj,  Xj,  nj  (2b) 

y  «  h(ut ,x, ,n)  +  Ht (x,  —  x,)  +  Lt(i±,  —  n)  (2c) 

where  yt  £  Rny  is  the  measurement  vector,  nt  £  Rlln  the 
measurement  noise  with  covariance  matrix  R,  which  is 
assumed  to  be  independent  of  vt,  h:  Rnx  x  Rnu  x  Rnn  ->  Rnu 
is  the  deterministic  nonlinear  measurement  model,  and 
H  _  dhjx,)  and  j  _M  _  are  the  Jacobians  of  the 

measurement  model  linearized  around  the  reference 
values  Xt  and  ll_ ,  respectively. 

UKF/SPKF 

The  EKF  only  uses  the  first  order  terms  of  the  Taylor 
series  when  expanding  the  nonlinear  functions.  As  such, 
the  linearization  validity  must  be  checked  to  avoid  large 
errors  built  up  in  the  estimated  posterior  distribution  of 
the  state  when  the  higher  order  terms  of  the  Taylor  series 
expansion  become  significant.  For  this  reason,  higher- 
order  EKF  and  iterated  EKF  have  been  proposed  in  the 
past. 


Unlike  the  EKF,  the  UKF  does  not  approximate  the 
nonlinear  dynamic  and  measurement  models;  it  utilizes 
the  true  nonlinear  models  but  instead  approximates  the 
distribution  of  the  state.  When  the  state  distribution  is 
Gaussian,  a  minimal  set  of  deterministically  chosen 
samples  (i.e.,  sigma  points)  can  be  used  to  completely 
capture  the  true  mean  value  and  covariance  of  the  state. 
These  sigma  points  then  propagate  through  the  true 
nonlinear  system  and  again  capture  the  posterior  mean 
and  covariance  accurately  to  the  second  order  for  any 
nonlinearity,  with  errors  only  introduced  in  the  third  and 
higher  orders.  In  comparison,  the  EKF  only  calculates  the 
posterior  mean  and  covariance  accurately  to  the  first  order 
with  all  high  order  moments  truncated. 

The  UKF  is  the  application  of  the  scaled  unscented 
transformation  (SUT)  [Julier,  2002]  to  recursive 
minimum  mean-square-error  (MMSE)  estimation. 
Consider  a  random  variable  x  £  R"  ~  /Mmx.  Px).  The 
covariance  matrix  is  scaled  up  by  a  factor  (n+K),  thus 
spreading  the  standard  deviation  by  a  factor  of  y  =  +  k  ■ 

Now  factorize  the  covariance  matrix  by,  say,  Cholesky 
decomposition,  producing  the  square  root  .  Construct 

2n+l  sigma  points  around  the  mean  vector  mx  with  the 
scaled  spread  square  root  y^~  as: 


X=  [mx,  mx+  y  ,  mx-  y  ^TfT] 


(3) 


Let  [X\i  be  the  ith  column  of  X  (i.e.,  the  i,h  sigma  point 
vector,  with  the  count  starting  from  0).  Propagating  it 
through  a  nonlinear  function  y  =  f(x)  produces  the 
posterior  sigma  point  vectors  as  []/];  =  f([Xh)  for  i  =  0  ... 
2n.  The  mean  and  covariance  are  calculated  from  the 
weighted  sample  mean  and  covariance  as: 


m,=i yrm 

i=0 

Pyy=lLWi(\yi -rny)(m-my)T 
1=0 

pv=HWi(lXl-rnx)(m-tny)T 

1=0 


(4a) 

(4b) 

(4c) 


where  W”  and  W,L  are  the  constant  weights  for  the  mean 
and  covariance,  respectively,  given  by: 

y»=  1  ,  i  ~  1,  2n  (5a) 

0  n  +  A  1  2  (n  +  A) 

1 V' =^-r  +  (l-a2+/9)’  W,c  =  W."  =  1,  ■■■,  2n  (5b) 

n  +  A 

where  X  =  a2(n+ic). 


The  parameter  k  >  0  is  chosen  to  ensure  the  positive  semi- 
definiteness  of  the  covariance  matrix,  which  defaults  to 
zero.  The  parameter  0  <  a  <  1  controls  the  size  of  the 
sigma  point  distribution  (probabilistic  spread  in  terms  of 
covariance)  and  takes  a  small  number  to  avoid  sampling 
non-local  effects  when  nonlinearities  are  strong.  The 
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parameter  P  is  a  weighting  term  used  to  incorporate  any 
knowledge  of  higher  order  moments  of  the  distribution. 
For  a  Gaussian,  the  optimal  choice  [3  =  2  [van  de  Merwe, 
2004], 

Figure  1  shows  the  propagation  of  a  random  variable 
through  a  nonlinear  function  with  the  SUT  (as  in  the 
UKF)  in  comparison  to  the  first  order  linearization  (as  in 
the  EKF).  Its  application  to  recursive  estimation  leads  to 
the  UKF,  which  includes  the  following  steps.  Step  1  - 
Construct  an  augmented  state  by  concatenating  the 
original  state  with  the  process  noise  and  measurement 
noise.  Step  2  -  Calculate  the  covariance  matrix  for  the 
augmented  state  and  select  the  sigma  points  for  the 
augmented  state.  Step  3  -  Conduct  the  time -update  by 
propagating  the  sigma  points  through  the  exact  nonlinear 
process  and  measurement  equations.  Step  4  -  Calculate 
the  covariance  matrix  of  the  predicted  measurements  per 
sigma  points  (Pyy)  and  the  cross-covariance  matrix 
between  the  predicted  measurements  and  the  augmented 
state  (PXy)-  Step  5  -  Conduct  the  measurement  update 
using  the  Kalman  filter  gain  as  JCV.  =  Pyy(Pxy)  *• 

Table  1  lists  the  algorithm  that  updates  the  mean  and 
covariance  of  the  Gaussian  approximation  to  the  posterior 
distribution  of  the  state.  As  shown,  there  is  no  explicit 
calculation  of  Jacobians  or  Flessians.  Since  the  covariance 
matrix  can  be  expressed  recursively  in  the  square-root 
form,  not  only  does  the  UKF  outperform  the  EKF  in 
accuracy  and  robustness,  it  does  so  at  no  extra 
computational  cost.  When  the  process  and  measurement 
noise  terms  are  purely  additive,  there  is  no  need  to 
augment  the  system  state  with  noise,  further  reducing  the 
computational  complexity. 


Figure  1  -  Propagation  of  Random  Variable  Through 
Nonlinear  Function:  UKF  vs.  EKF 

Particle  Filter  and  Hybrid  Implementation 

Both  the  EKF  and  UKF  reply  on  the  Gaussian  assumption 
and  its  approximation.  The  particle  filtering  method 
described  in  this  section,  however,  does  not  require  this 
assumption.  But  it  has  problems  of  its  own.  To  overcome 


such  problems,  the  particle  filter  may  be  combined  with 
an  EKF  or  UKF  in  a  hybrid  manner.  The  design  of  a 
particle  filter  is  based  on  four  basic  ideas,  which  are 
discussed  below. 


Discrete  Approximation  of  PDF 

The  first  idea  is  the  approximation  of  a  continuous- 
support  distribution  p(x0:t|yI:t)  by  N  discrete  samples  x0:t  1 , 
“randomly”  drawn  from  the  distribution  p(xot|yi  t),  for  i  = 
1,...,N: 

p(*o,  I  y-u )  =  ^7  X  8  ( x« « _  4? )  ^ 

Ntt 


where  the  subscript  “0:t”  or  “l:t”  indicates  the 
observation  interval  from  0  or  1  to  t  and  5(»)  is  the  Dirac 
delta  function.  This  is  the  so-called  Monte  Carlo  method. 
With  this  approximation,  the  computation  of  expectation 
of  any  function  of  x0:t,  g(»),  is  reduced  from  a  complicated 
integration  to  a  simple  summation  as: 

E{g,(x  0:,)}  =  f  g,(*0lM*0J  I  A, )<&»:,  =  ^ 


Concatenation: 

Augment  the  state  vector  to  include  noise  terms: 
a  r  T  T  T  ,r 

X ,  =  [x,  V,  n,  J  ,na  =  nx  +  nv  +  n„ 

Initialization: 

x0  =  E{x,},  P0  =  E{(x0  -X0  )(x0  -x0  /} 

£o  =  Efx o  }=[xa0, 0.  0J,  Pa0  =  E{(X “  -X°Q  )(xaQ  -X*  fj  =  diag{P0,  Q.  R} 

Recursive  over  t: 

Calculate  Sigma  Points 

Xf_x  =  [ X„V, .  %\j-\  >  ■•■>  X 2 na  f-i  ]  where 

X0",_,  =  x"_,  ,  IF<0m)  =  M(nx+  X),  Wic)  =  A /(n;+  X)  +  (1-cC+p) 

=x“_,  +  [j(na  +/c)P,a_i  Jo  i=  I,  ....  na 

X“t_x  =x“_,  -[-\(na  ] 0i  =  na+l . 2na 

w(m)  =  w<c)  =  I/(2(„  +  X)),  i  =  I ,  ....  2n  a 

Time  Update 

x,;_t  = 

Ua  =±wrx*At_x 
/= 0 

A-. = Z’f 
(=0 

Vjh 

2na 

y  =y  w(m)y 

Measurement  Update 

r  =yw,L'iy  -y  iry  -y  f 

-’r y,  i—,  ‘  L^L/,/|/-l  JL^l_/,/|l-l  A/U-l J 

1=0 

pv, 

i=0 

X,  =pxy(p~~yl 
i,  =I,m  +X,(y,  -Vf.A 

P,  =Pttx-X,PMXJ 

Table  1  -  Unscented  Kalman  Filter 


Importance  Sampling 

The  second  idea  is  the  importance  sampling.  In  the 
estimation  problem,  the  posterior  distribution  p(x0:t|y i:t)  is 
in  fact  what  we  want  to  estimation  from  data,  thus  not 
available  for  sampling  directly.  One  way  to  get  around 
this  is  to  approximate  the  expectation  over  the  unknown 
distribution  p(x0:t|y i:t)  by  another  expectation  taken  over  a 
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known  easy-to-sample  distribution  q(x0;t|yi:t),  called 
importance  function  ,  also  known  as  proposal  distribution 
(which  we  will  use  interchangeably): 


EIS,W.) 

=  js,(X  »:,) 


?(*0:<  I  y \:l  ) 
pUb  I  xm)p(xM)l p(yu) 


=  |  g,  (*(,:,) 


P(y  1:,) 


q(*o,  \yu) 

I  y^)dx a, 


?(•*(>:<  lyi:,)*0:, 
?(*<>:,  bu)*®, 


(8) 


where  the  variable  wt(x0:t)  is  the  unnormalized  importance 
weight  defined  by: 

(  )  =  /KjU*o,)jK*o,)  (9) 

?(•«»,  I  v1:, ) 

The  probability  density  function  p(y1:t)  in  the  denominator 
of  Eq.  (8)  can  be  evaluated  as: 

p(y,,)  =  f p(yu  I *o ■j)p(.x<s,)q('*°'J 

=  J  w,(xM)q(.x 0J  |  y1:, )(fe0;(  =  £9{w,(v0:,)}  (10) 


corresponds  to  a  Markov  process  (i.e.,  p(x0:t)  = 
p(x0)ntj=ip(xj|xj.i)),  the  observations  are  conditionally 
independent  given  the  state  (i.e.,  p(yi:t|x0;t)  =ntj=ip(yj|xj)), 
and  the  importance  function  or  proposal  distribution  is 
factorable  (i.e.,  q(x0:,|yi:t)  =  q(x0:t- i |y i :t- i )q(xt|x0:t_ i ,y i :t- i ) 
according  to  the  Bayes’  rule),  then  the  unnormalized 
importance  weight  can  be  estimated  recursively  as: 


w,  (*<»)  = 


(PUtM  \xM.l)p(yt  \x,))(p(x0.J_l)p(x,  I  xt_j) 


\ya-i)q(x,  \x0 :(_i,y1:f) 
p(yr  \x,)p(x,  |x,_t) 

<l(X,  I  W0:,_, ,  Vi:r  ) 


(12) 


Eq.  (12)  provides  a  mechanism  to  sequentially  update  the 
importance  weight  given  the  conditional  proposal 
distribution  q(xt|x0:t-i,yi:t).  Indeed,  we  can  sample  from 
this  proposal  distribution  (i.e.,  generate  N  discrete 
samples  xu\  according  to  q(xt|x0:t-i,yi:t))  and  evaluate  the 
likelihood  and  transition  probabilities  (i.e.,  p(yt|xt)  and 
p(xt|xt_i)  given  by  the  process  and  measurement  models  of 
Eqs.  (la)  and  (2a),  respectively)  for  theses  samples.  Table 
2  lists  the  algorithm  for  a  generic  particle  filter  with 
additional  steps  discussed  below. 


where  Eq{»}  is  the  expectation  taken  over  the  importance 
function  or  proposal  distribution  q(x0:t|yi:t)- 


Following  Eq.  (10),  p(yi:t)  =  Eq{wt(x0:t)}  can  be  taken  out 
of  the  integral  of  Eq.  (8),  still  in  the  denominator,  though. 
By  consequence,  the  numerator  of  Eq.  (8)  can  be 
evaluated  over  q(x0:t|yi:t)  in  the  same  manner,  leading  to 
Eq{gt(x0:t)w,(Xo:t)}. 

When  these  expectations  are  approximated  as  in  Eq.  (6) 
by  drawing  N  samples  from  the  importance  function 
(proposal  distribution)  q(x0:t|yi:t),  we  have: 


E{g,(x0:,)}  = 


=  'Eg,(Xo:,>) 


vXwi(x!°) 


w,(xj0) 


vZw,(X(W) 


j= 1 


(11a) 

(lib) 


The  normalized  importance  weight  W(tl)  is  used  in  the  last 
equality  of  Eq.  (11a)  with  ^  -a,  =  ^ . 


Sequential  Importance  Sampling  (SIS) 

The  third  idea  is  the  sequential  importance  sampling 
(SIS).  Under  the  assumptions  that  the  underlying  state 


Table  2.  Generic  Particle  Filter 


Initialization:  t  -  0  |  Draw  lhe  initial  state  x<0  from  the  prior i  =  j . N 

Fort=  1,2,  ... 

Importance  Sampling 

Sample  x'0  ~  q(x,  |  x^_t ,  yVj )  and  set  *  ]  f°r '  =  1-  ....  AI 

Evaluate  the  unnormalized  importance  weight: 

,0  (o  P(y,  1  *,)/>(*,  l*,-i )  ,  .  , 

?(*,  1^,'nTiq) 

Normalize  the  importance  weights: 

=xfd>:l)T  f <xt-i » 

Selection  (Resampling): 

Multiply  (or  suppress)  samples*'1*  with  high  (or  low)  importance  weights  VV,(,>  to  obtain 
N  random  samples  *q!*  approximately  distributed  according  to  pfx^.j  \yr) 

Set  wj°  =  vv,(i)  =  1/N  for  i  =  1, ...,  N 

Output: 

The  output  of  the  algorithm  is  a  set  of  samples*^!,*  with  weights  w'1*  =1/N  for  i  =  1,  ....  N 

The  samples  can  be  used  to  approximate  the  posterior  distribution  as 

P(xo,  1  yt, )  =  P(xo,  lTi:,)  =  ^Z S(-x*j  ~ ) 

The  samples  can  also  be  used  to  calculate  expectation  for  appropriate  functions  as: 

E {§,  (*0:,  )}  =  J g,  (Xto  )p(x0:,  1  yu  )dxQj  (xg  ) 

which  is  the  marginal  conditional  mean  of  X0  l  when  g,  (*0., )  =  X0.:  and  the  marginal 
conditional  covariance  of  x0j  when  g, (*0:, )  =  —  E p^x {*<)*}  . 

Resampling  and  Diversification 

For  any  practical  implementation,  the  number  of  samples 
that  can  be  drawn  from  a  distribution  is  limited.  As  such, 
the  choice  of  importance  function  or  proposal  distribution 
becomes  critical,  creating  other  issues  impeding  the 
success  of  a  particle  filter. 

An  easy  choice  of  importance  function  for  a  process 
model  with  additive  Gaussian  noise  is  the  transition  prior: 

q(xt\x0:t-i,yi:,)  ~p(x,\xtfi  ~  Nfffu^x^O),  Q,fi  (13) 

However,  this  proposal  distribution  does  not  incorporate 
the  latest  data  available  and  it  runs  the  risk  to  deplete  the 
samples  in  the  sense  that  after  a  few  iterations,  one  of  the 
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normalized  importance  weights  tends  to  1  while  the 
remaining  weights  tend  to  0.  This  effectively  removes  a 
large  number  of  samples  from  the  sample  set  because 
their  importance  weights  are  numerically  insignificant. 

To  avoid  this  degeneracy,  the  fourth  idea  is  the 
importance  resampling,  also  called  selection,  in  which 
samples  with  low  importance  weights  are  eliminated 
while  samples  with  high  importance  are  multiplied, 
keeping  the  total  population  of  samples  in  the  same  level. 
Techniques  for  resampling  include  sampling  importance 
resampling  (SIR),  residual  resampling,  and  minimum 
variance  sampling. 

Since  the  selection  step  favors  the  creation  of  multiple 
copies  of  the  “fittest”  particles  (thus  allows  us  to  track  the 
updated  distributions),  many  “unfit”  particles  may  end  up 
with  few  or  none  copies,  leading  to  sample 
impoverishment.  To  solve  this  problem,  an  additional  step 
is  therefore  needed  to  introduce  the  sample  diversification 
after  the  selection  step  without  affecting  the  validity  of 
the  approximation.  A  brute  force  approach  would  increase 
the  number  of  samples.  But  a  refined  technique  is  to 
implement  a  Markov  chain  Monte  Carlo  (MCMC)  step, 
which  moves  new  particles  to  areas  of  more  interest  in  the 
state  space  by  applying  a  Markov  chain  transition  kernel 
[Ruanaidh  and  Fitzgerald,  1996].  Figure  2  illustrates  this 
process. 


Initialization 


Weight  Update 


Resampling 


Diversification 


Figure  2-  Resampling/Selection  and  Diversification 

Hybrid  Kalman  Particle  Filter  to  Move  Proposal  Density 
Closer  to  Likelihood  Function 

One  cause  for  degeneracy  frequently  encountered  when 
using  importance  functions  as  Eq.  (13)  is  that  the  state 
transition  prior  and  measurement  likelihood  density 
functions  do  not  overlap  (either  too  peaked  or  widely 
separated).  This  problem  can  be  mitigated  by  moving 
particles  to  areas  of  high  likelihood.  One  way  to  do  so  is 
to  use  a  separate  EKF  to  generate  and  propagate  a 
Gaussian  proposal  distribution  for  each  particle: 


~(0 


>(0 1 


Since  the  UKF  is  more  accurate  than  the  EKF,  thus 
having  a  more  accurate  importance  function  with  bigger 
support  overlap  with  the  true  posterior  distribution,  the 

estimates  and  Pt from  an  UKF  can  be  used  to 

generate  the  proposal  distribution  as  in  Eq.  (14).  In  this 
way,  the  EKF  or  UKF  is  effectively  combined  with  a 
particle  filter,  yielding  an  extended  Kalman  particle  filter 
(EKPF)  or  an  unscented  Kalman  particle  filter  (UKPF). 


It  is  clear  that  EKPF  and  UKPF  require  very  high 
computational  power  particularly  for  systems  of  large 
dimensions.  To  strike  a  balance  between  performance  and 
computation,  the  Gaussian  mixture  sigma  point  particle 
filter  (GMSPPF)  combines  an  importance  sampling  (IS) 
based  measurement  updating  with  a  scaled  unscented 
transformation  (SUT)  based  Gaussian  sum  filter  for  the 
time  updating  and  proposal  density  generation  as  shown 
in  Figure  3. 


EKF/UKF 
Measurement 
Update  for 
Proposal 
Density 
Generation 


EKF/UKF 
-Based  Time 
Propagation 
For  Each 
Gaussian 


Figure  3  -  Block  Diagram  of  Hybrid  Kalman  Particle  Filter 

In  GMSPPF,  the  IS-based  measurement  updating 
produces  the  weighted  posterior  particle  set.  It  is  followed 
by  the  resampling  and  diversification  steps.  The  particles 
are  then  represented  by  a  finite  Gaussian  mixture  model 
found  using  the  expectation-maximization  (EM) 
algorithm.  The  resampling  and  diversification  step  may  be 
omitted  if  a  weighted  EM  algorithm  is  used  in  generating 
the  Gaussian  mixture  thanks  to  its  inherent  kernel 
smoothing  nature. 

EKF  or  UKF/SPKF  can  be  used  to  time-propagate  and 
then  measurement-update  each  particle  or  each 
component  of  the  Gaussian  mixture.  The  posterior 
Gaussian  mixture  is  then  used  as  the  proposal  density  to 
draw  particles.  In  this  way,  each  measurement  is  used 
twice,  the  first  time  by  an  EKF  orUKF/SPKF  to  bring  the 
measurement-updated  importance  function  closer  to  the 
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likelihood  function  and  the  second  time  by  the  importance 
sampling-based  update  to  produce  more  accurate  estimate 
of  the  posterior  distribution.  Other  implementation 
schemes  of  particle  fdter  can  be  found  in  [Doucet,  de 
Freitas,  Gordon  2001;  Liu,  2001;  Ristic,  Arullampalam 
and  Gordon,  2004]. 

SIMULATION  SCENARIO  AND  FILTER  MODELS 

A  simple  tracking  scenario  is  shown  in  Figure  4  where  a 
satellite  moves  along  a  perfect  circle  of  radius  rs  = 
26,560km  at  a  constant  angular  velocity  of  a>s  =  360°/12hr 
around  the  earth.  The  truth  position,  velocity,  and 
acceleration  components  are  generated  by: 


0(t)  =  0O  -  cot,  ra  =  360/12  (°/hr)  (15a) 

x(t)  =  rscos(0(t)),  y(t)  =  rssin(0(t))  (15b) 

vx(t)  =  rscosin(0(t)),  vy(t)  =  -rscocos(0(t))  (15c) 

ax(t)  =  -co2x(t),  ay(t)  =  -co2y(t)  (15d) 


With  the  notations  r=  a  =  [ax,  ay]’  and  r  =  [x,  y]’,  Eq. 
(15d)  can  be  written  into  the  form  of  gravitational 
acceleration: 


where  |i  =  k2M  =  3,986*  108m3/s2  is  the  earth’s 
gravitational  constant,  k2  is  the  universal  constant  of 
gravitation,  and  M  is  the  mass  of  the  earth.  The  equation 
holds  because  a  =  k~M/r  =  V'/r  =  orr,  which  leads  to  to 
=  a/r  =  p/r3. 

Monitor  stations  on  the  surface  of  the  earth  with  radius  re 
=  6,378km  are  assumed  to  he  within  the  orbital  plane, 
thus  making  it  a  2D  scenario.  The  local  horizon 
established  for  the  monitor  station  makes  about  14°  with 
the  x-axis  in  either  direction,  above  which  the  satellite  is 
visible  to  the  monitor  station.  Since  we  will  apply  the 
kinematic  method  for  orbit  determination,  the  formulation 
is  thus  done  in  the  earth-fixed  coordinates. 

The  pseudorange  measurement  denoted  by  r  for  the 
monitor  station  located  at  (xm,  ym)  is  related  to  the  satellite 
position  (x,  y)  in  a  nonlinear  manner  as: 

r  =  tJ(x  ~xm)2  +  (y  -  ym  )2  +nr  (17) 

where  n,  is  the  measurement  noise  with  variance  o,2. 

The  measurement  noise  variance  is  set  to  be  a,2  =  (0.1m)2 
in  the  simulation,  used  by  both  the  filters  and  in  the 
measurement  generation.  Dual-frequency  GPS  receivers 
with  P-code  ranging  have  their  pseudorange  error 
variance  around  (10m)2  whereas  carrier  phase  ranging 
measurements  have  their  error  variance  around  (0.01m)2 
or  better.  This  choice  for  our  simulation  is  “arbitrary”  for 
the  sake  of  convenience. 


The  2D  satellite  motion  is  considered  by  the  tracker  filter 
as  two  independent  3rd  order  models  with  the  position, 
velocity,  and  acceleration  as  their  state  for  each  axis  as: 


Figure  4  -  Simple  2D  Tracking  Geometry 
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where  T  =  Is  is  the  sampling  interval  and  the  process 
noise  wx(k)  and  wy(k)  are  random  accelerations  modeled 
as  white  Gaussian  with  their  variances  ct^2  and  ow y2, 
respectively.  In  generating  the  truth  orbit,  we  do  not 
include  any  process  noise  (purely  deterministic). 
Flowever,  the  tracking  filter  will  include  a  process  noise 
with  the  variance  cr^2  =  a^2  =  (0.1m/s2)2  for  the 
kinematics  model  of  Eq.  (18). 

An  orbiting  satellite  is  under  pulling  force  that  changes  all 
the  time.  For  the  orbit  arc  in  Figure  4,  vx  is  always 
positive,  changing  from  the  initial  value  of  l,000m/s  to 
the  maximum  of  3,874m/s  and  down  to  l,000m/s  again 
whereas  vy  changes  from  near  4,000m/s  to  near  - 
4,000m/s.  The  tangential  linear  velocity  is  a  large  number 
of  3,874m/s.  The  acceleration  components  ax  and  ay 
change  about  0.5m/s2  over  half  of  the  two-hours  arc.  Over 
a  short  period  of  time,  it  can  be  considered  constant 
provided  that  the  process  noise  is  properly  adjusted  to 
account  for  the  changes.  As  such,  the  linear  process 
model  of  Eq.  (18)  in  the  Cartesian  coordinates  is 
appropriate  for  orbit  determination  when  accurate 
measurements  are  available  at  high  sampling  rate  and  but 
it  does  not  provide  any  prediction  capability. 

Any  tracking  filter  that  is  based  on  the  model  of  Eq.  (18) 
alone  does  not  take  advantage  of  the  well-behaved  motion 
pattern,  which.  Furthermore,  if  there  is  only  one  monitor 
station  with  range  measurements,  the  problem  of 
geometric  dilution  of  precision  (GDOP)  appears.  In  our 
simulation,  we  therefore  consider  two  monitor  stations, 
which  are  located  at  (xm,  ym))  =  (-re,  0)  and  (0,  re), 
respectively.  The  arc  of  satellite  orbit  that  is  visible  to 
both  stations  is  from  166°  to  104°  in  the  second  quadrant. 
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SAMPLE  BEHAVIORS  AND  ANALYSES 

Four  nonlinear  filters,  namely,  EKF,  UKF,  PF,  and 
GMSPPF,  are  implemented  with  sample  behaviors 
presented  here  for  visualization  and  comparison.  In  the 
simulation,  the  initial  conditions  are  set  as: 

x0=x0  +  [100,  10,  1,  100,  10,  1]T  (19a) 

P0  =  diag{[1002,  102,  l2,  1002,  102,  l2]}  (19b) 

for  all  the  nonlinear  filters  except  for  the  particle  filter  for 
the  reasons  to  be  explained  later. 

Figures  6,  10,  14,  and  18  show  the  true  and  estimated 
orbits  for  the  four  filters,  EKF,  UKF,  PF,  and  GMSPPF, 
respectively.  Except  for  the  PF,  the  other  three  filter 
estimates  are  quite  close  with  an  approximate  ranking  of 
performance  as  EKF  <  UKF  <  GMSPPF. 

Figures  7,  11,  15,  and  19  show  the  true,  measured,  and 
estimated  ranges  (the  top  plot)  and  the  range  residual 
errors  (the  bottom  plot)  for  the  four  filters  as  seen  from 
monitor  station  1.  Figures  8,  12,  16,  and  20  show  the  true, 
measured,  and  estimated  ranges  (the  top  plot)  and  the 
range  residual  errors  (the  bottom  plot)  for  the  four  filters 
as  seen  from  monitor  station  2.  Since  the  measurement 
noise  standard  deviation  is  set  to  be  0.1m,  it  is  reasonable 
to  see  that  the  range  residual  errors  are  also  around  0.1m. 

Figures  9,  13,  17,  and  21  show  the  estimation  errors  for 
position  (top),  velocity  (middle)  and  acceleration  (bottom) 
for  the  four  filters.  It  can  be  seen  that  the  estimates  of  the 
x-components  are  consistently  better  than  the  y- 
component  counterpart  over  this  short  orbit  arc  because  of 
the  influence  of  GDOP. 


Figures  25  and  26  show  the  selection  of  initial  sigma 
points  for  the  UKF.  The  augmented  state  vector  of  the 
UKF  includes  the  position  (2),  velocity  (2),  acceleration 
(2),  process  noise  (2),  and  measurement  noise  (2).  The 
total  dimension  is  thus  n  =  10  and  the  number  of  sigma 
points  is  then  2n  +  1  =21. 

Since  we  plot  a  subspace  of  dimension  2  at  a  time  per 
figure,  we  can  only  show  five  distinctive  sigma  points  per 
subspace,  which  are  located  at  the  center  and  two 
extremes  of  the  major  and  minor  axes,  respectively,  of  the 
ellipse.  The  parameters  used  for  the  unscented 
transformation  are  a  =  1,  P  =  2,  and  k  =  0.  The  resulting 
radius  is  about  3cr,  consistent  with  the  spreading  factor  y  = 
VTo  ~  3. 

Figure  27  shows  the  initial  1,000  particles  drawn  from  the 
initial  distribution  for  the  PF.  Its  processing  results  are 
presented  in  Figures  14  through  17.  With  1,000  particles, 
the  PF  has  to  be  initialized  with  much  tighter  conditions 
than  the  other  three  filters  given  in  Eq.  (19)  as: 

i#=  3o +  [10,  0.1,  0.01,  10,0.1,0.01]’  (22a) 

Po  =  diag{[102,  0.12,  0.012,  102,  0.12,  0.012]}  (22b) 

Otherwise,  the  filter  starts  to  diverge  after  few  steps.  The 
particular  PF  algorithm  implemented  in  the  simulation  is 
the  so-called  bootstrap  algorithm,  also  known  as  the 
condensation  algorithm.  As  one  of  the  rudimentary 
particle  filters,  it  is  known  for  such  problems  as  sample 
impoverishment  (depletion)  among  others. 

In  this  PF  algorithm,  the  proposal  density  is  Eq.  (13)  and 
the  particle  weight  updating  is  simplified  into: 


To  appreciate  this,  we  run  simulations  over  longer  periods 
of  time,  which  is  increased  from  100s  in  the  previous 
simulations  to  the  entire  arc  of  7466s  (about  2  hrs).  Only 
the  results  for  UKF  are  shown  here.  Figure  22  shows  the 
true  and  estimated  orbit  arc  and  Figure  23  shows  the 
estimation  errors  for  position  (top),  velocity  (middle),  and 
acceleration  (bottom). 

As  the  satellite  moves  away  from  the  negative  x-axis 
toward  the  positive  y-axis  (clockwise),  the  GDOP 
degrades  for  the  x-component  but  improves  for  the  y- 
component.  This  is  visible  in  the  top  plot  of  Figure  23  as 
the  x  position  error  grows  while  the  y  position  error  is 
reduced.  The  position  error  ratio  changes  from  Ax:Ay  = 
1:4.5  to  4.5:1,  consistent  with  what  is  shown  in  Figure  24 
for  the  GDOP  calculated  as: 


[DOPx,  DOPy]  =  diag {^H'Hy'  } 

H__dh(x)  =  ( X-X ,)/n  (y-yj/r, 
x  |_(x  — x2)/  r2  (y-y1)/r1 


(20a) 

(20b) 


where  (xb  yi)  and  (x2,  y2)  are  the  locations  of  the  monitor 
stations  with  their  ranges  to  the  satellite  being  ly  and  r2, 
respectively. 


w‘k  =  wVixpfeljcV.i) 

=  wV,x.W]zk  -  h(uk,  Ak|k-i,0);  Rk)}  (23) 

which  is  the  product  of  the  old  weight  and  the  likelihood 
value  of  the  latest  observation  given  the  propagated 
particle.  The  problem  stems  from  the  calculation  of 
likelihood  values.  In  the  EKF  and  UKF,  the  innovations 
(i.e.,  the  measurement  prediction  errors),  no  matter  how 
large  it  may  be,  are  weighted  by  the  Kalman  filter  gain 
determined  by  the  product  of  the  predicted  state 
estimation  error  covariance  (APA’+Q)  and  the  inverse  of 
the  measurement  error  covariance  (HPH’+Q)"1. 

For  our  measurement  model  with  a,,2  =  (0.1m)2,  if  the 
orbit  position  errors  are  larger  than  10m,  the  measurement 
prediction  errors  (or  the  innovations)  can  be  larger  than 
10m  (further  contributed  by  velocity  and  acceleration 
errors).  This  is  equivalent  to  an  error  of  100ctv  in  terms  of 
the  measurement  noise  standard  deviation.  The  resulting 
likelihood  values  are  practically  zero. 

If  the  initial  state  errors  are  large  and  few  or  none  of  the 
initial  particles  drawn  are  near  the  true  state  within  10m 
or  closer,  two  phenomena  have  been  observed.  One  is  that 
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only  one  or  few  particles  survive  the  updating  while  all 
the  rest  are  rendered  zero.  The  PF  algorithm  collapses 
because  there  are  no  sufficient  particles  to  continue.  The 
second  is  that  all  likelihood  values  are  close  to  zero  and 
practically  the  same  and  by  consequence,  they  produce  no 
meaningful  changes  to  the  weights,  thus  making  the 
updating  process  ineffective  (as  if  no  measurement 
update). 

To  avoid  the  problem,  one  can  either  use  smaller  initial 
errors  or  a  larger  amount  of  particles.  The  latter  may 
amount  to  excessive  computation.  We  adopted  the  former 
technique  in  the  present  simulation  with  the  small 
initialization  errors  as  given  by  Eq.  (22).  Actually,  various 
techniques  have  been  proposed  to  perform  on-line 
monitoring  and  editing  [Gordon,  Salmond,  and  Smith, 
1993;  Gordon,  Salmond,  and  Ewing,  1995]. 

To  illustrate,  we  now  look  at  the  resampling  process  of 
the  particle  filter  at  the  100th  time  step.  As  shown  in 
Figure  28,  the  top  plot  is  the  initial  weights  from  the  99th 
step  in  which  all  particles  are  assigned  with  an  equal 
weight  of  1/N  with  N  =  1000  after  resampling  at  the  step. 

At  Step  100,  N  samples  are  first  drawn  from  the 
distribution  of  the  process  noise.  These  noise  samples  are 
used  together  with  the  state  samples  (i.e.,  the  particles  of 
Step  99)  to  predict  the  state  at  Step  100  according  to  Eq. 
(18).  The  results  are  shown  in  the  middle  plot  of  Figure 

28.  Note  that  since  the  process  noise  variance  is  set  to  be 
very  small  (0.1m/s2)2,  what  we  see  in  Figure  28  is 
essentially  the  prediction  driven  by  the  velocity  and 
acceleration  estimates. 

The  bottom  plot  of  Figure  28  shows  the  likelihood  values 
for  these  predicted  particles  given  an  observation.  It  has  a 
Gaussian  shape. 

The  updated  weights  are  shown  in  the  top  plot  of  Figure 

29,  which  is  the  normalized  product  between  the  old 
weights  and  likelihood  values.  This  also  has  a  Gaussian 
shape.  The  middle  plot  of  Figure  29  shows  the  state 
transition  probability  or  the  prior  distribution  of  the  state 
given  the  predicted  state  and  estimated  state  of  the 
previous  step.  In  this  particular  simulation  run,  the 
resampling  threshold  is  set  to  be  N,  a  typical  number  used 
by  the  bootstrap  algorithm.  Since  the  effective  number  of 
particles  is  always  less  than  N,  resampling  is  done  every 
step.  The  weights  after  resampling  are  again  equal  to  1/N, 
as  shown  in  the  bottom  of  Figure  29. 

Figure  30  shows  the  particle  indices  before  and  after 
resampling.  It  has  some  discontinuities  and  horizontal 
segments.  The  latter  indicates  that  those  particles  with 
large  weights  got  multiplied  during  resampling.  This  is 
consistent  with  thegrouping  seen  in  the  top  plot  of  Figure 
29  for  the  weights  prior  to  resampling. 


To  avoid  depletion  of  particles  or  impoverishment,  one 
technique  is  to  redraw  particles  from  an  analytic 
distribution  estimated  from  data  as  done  in  GMSPPF  with 
a  Gaussian  mixture  shown  in  Figures  18  to  21.  Additional 
simulation  results  can  be  in  [Yang,  2004]. 

MONTE  CARLO  SIMULATION  RESULTS  AND 
ANALYSES 

The  Monte  Carlo  simulation  results  are  presented  in  this 
section.  For  easy  comparison,  we  use  the  same  simulation 
scenarios  and  the  same  initial  conditions  for  the  four 
nonlinear  filters.  While  the  initial  conditions  were  held 
the  same,  the  observation  noise  was  different  as  it  was 
drawn  from  the  random  noise  generator  in  each  Monte 
Carlo  run.  Similarly,  the  state  particles  were  drawn 
differently  for  each  run. 

In  the  first  case,  we  conducted  500  “independent”  Monte 
Carlo  runs  with  1000  particles  over  500  time  steps  (one 
second  per  step).  Figure  32  shows  the  root  mean  square 
(RMS)  errors  of  the  measurement  residuals  at  two 
monitor  stations.  The  RMS  value  for  an  error  is  calculated 
using  the  following  recursive  formula: 

RMSe2(n)  =  (1-1/n)  RMSe2(n-l)  +  (l/n)e2(n) 

RMSe(0)  =  0,n=  1,2,  ...  (24) 

where  e(n)  is  the  error  term  of  a  variable  of  interest  at  the 
nth  Monte  Carlo  run  and  RMSc(n)  is  the  resulting  RMS 
value  including  that  run. 

In  the  following  plots,  we  select  not  to  show  the  results  of 
the  particle  filter  (the  bootstrap  algorithm)  because  it 
required  a  different  initialization  as  explained  previously. 
In  addition,  we  observed  some  runs  with  divergence  for 
the  PF  without  applying  any  on-line  editing  as 
recommended  in  the  original  papers  [Gordon,  Salmond, 
and  Smith,  1993;  Gordon,  Salmond,  and  Ewing,  1995] 
and  this  sort  of  instability  never  occurred  to  the  other 
three  filters.  Otherwise,  the  RMS  errors  values  for  the  PF 
are  similar  to  the  UKF  and  GMSPPF. 

As  shown  in  Figure  32,  the  RMS  errors  for  the  UKF  and 
the  GMSPPF  are  on  the  same  level  (better  than  0.09  m) 
while  the  RMS  errors  for  the  EKF  are  on  the  order  of  0.1 
m.  It  is  reasonable  for  the  EKF  because  the  measurement 
noise  was  set  with  a  standard  deviation  of  0.1  m. 
However,  it  is  somehow  against  intuition  to  see  that  the 
measurement  residuals  for  the  UKF  and  GMSPPF  are 
slightly  below  the  noise  level.  It  remains  to  determine  if 
this  is  because  the  filters  “follow”  the  noise  and/or  have 
some  variance  reduction  capability  due  to  nonlinear 
averaging. 

Figures  33  and  34  show  the  RMS  values  of  the  position 
estimation  errors  in  X  and  Y,  respectively.  It  can  be  seen 
that  after  the  initial  transient  periods  (due  to  large  initial 
estimation  error  covariance  and  filter  gain),  the  estimation 
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error  RMS  values  remain  consistent.  The  changes  in 
position  error  RMS  values  are  caused  by  GDOP  (see 
Figure  24).  In  the  X-dimension,  the  GDOP  is  initially 
close  to  1  and  grows  up  to  4.5  after  7500  s.  For  the  first 
500  steps  (which  started  from  100s)  as  shown  in  Figure 
33,  the  initial  position  error  RMS  is  close  to  0.1  m.  When 
compared  to  the  measurement  error  standard  deviation,  it 
represents  a  GDOP  of  1,  as  predicted  by  the  geometry. 

In  the  Y-dimension,  the  GDOP  is  initially  close  to  4.5  and 
decreases  down  to  1  after  7500  s  as  shown  in  Figure  24. 
For  the  first  500  steps  as  shown  in  Figure  34  (which 
actually  started  from  100  s),  the  initial  position  error  RMS 
is  about  0.43  m  for  EKF  and  0.35  m  for  UKF  and 
GMSPPF.  When  compared  to  the  measurement  error 
standard  deviation,  it  represents  a  GDOP  of  4.3  and  3.5 
for  EKF  and  UKF/GMSPPF,  respectively.  The  EKF, 
though  producing  larger  errors,  is  closer  to  the  value 
predicted  by  the  geometry. 

Figures  35  and  36  show  the  RMS  values  for  the  velocity 
errors  in  vx  and  vy,  respectively.  The  RMS  values  were 
calculated  over  95  Monte  Carlo  runs  but  during  7000  time 
steps  (covering  almost  the  half  sky).  The  velocity  error 
RMS  values  for  UKF  and  GMSPPF  change 
“monotonically”  and  are  consistent  with  GDOP.  That  is, 
the  vx  RMS  grows  up,  as  the  X-GDOP  gets  worse.  At  the 
same  time,  the  vy  RMS  improves  as  the  Y-GDOP 
becomes  smaller. 

However,  the  velocity  error  RMS  values  for  EKF  are 
more  ‘‘‘dramatic”  in  the  sense  that  the  vx  RMS  grows  up 
exponentially  rather  than  near-linearly  with  the  X-GDOP. 
In  addition,  the  vy  RMS  exhibits  a  full  cycle  oscillation. 
This  periodic  behavior  is  somewhat  correlated  to  the 
underlying  velocity  values.  For  the  simulation  considered, 
vx  is  always  positive  whereas  vy  changes  sign  at  the  mid¬ 
point. 

We  only  considered  about  two  hours  of  operation.  When 
the  simulation  is  done  for  the  full  orbit  evolution,  periodic 
behaviors  are  expected.  Indeed,  this  has  been  observed  in 
the  GPS  OCS  Kalman  filter  with  dynamics  models 
[Brown,  1991].  For  GPS,  the  state  and  measurement 
partials  and  the  filter  gains  are  periodic  with  the  period 
being  a  sidereal  day.  The  modeling  errors  in  f(x)  and  h(x) 
are  therefore  repeatable  per  sidereal  day  assuming  the 
same  accuracy  from  day  to  day.  As  a  result,  the  actual 
estimation  errors  induced  by  modeling  errors  in  the 
functions  f(x)  and  h(x)  will  become  periodic. 

In  our  simulation,  the  state  model  used  by  all  filters  is  a 
linear  kinematic  model  with  constant  velocity  and 
acceleration  per  updating  interval.  Only  the  measurement 
equations  are  nonlinear.  The  EKF  suffers  from 
linearization  errors  while  both  the  UKF  and  GMSPPF 
make  use  the  nonlinear  equations  directly. 


The  GMSPPF  has  smaller  RMS  values  than  the  UKF  but 
they  are  so  minuscule  to  be  negligible.  We  also  tested 
other  cases.  This  included  (1)  different  initial  conditions 
with  up  to  ten  times  errors  and  (2)  large  number  of 
particles  (up  to  10000),  which  improved  the  results 
somewhat  for  the  bootstrap  filter  but  not  very  much  for 
the  GMSPPF  (considerably  slower  though).  The  results 
remain  similar. 

In  the  GMSPPF  implementation,  we  tried  three  to  five 
Gaussian  mixture  components  in  each  step  to  fit  to  the 
particle  clouds.  The  initial  mixture  component  parameters 
are  obtained  using  the  k-mean  clustering  algorithm.  The 
actual  fitting  is  done  using  the  expectation-maximization 
(EM)  algorithm.  The  iteration  is  stopped  when  the  log 
likelihood  values  between  two  iterations  drops  below 
0.001  or  the  number  of  iterations  exceeds  10. 

To  illustrate,  Figure  37  shows  the  result  of  one  fitting 
example  in  a  two-dimensional  state  space  (i.e.,  position  s 
vs.  velocity  S  )  with  1000  particles.  The  resulting  three 
mixture  components  have  weights  of  0.45064,  0.37583, 
and  0.17353,  respectively.  The  3a  ellipses  are  aligned 
almost  with  the  axes,  indicating  small  correlation  between 
the  two  variables  at  the  start  of  simulation. 

Figure  38  shows  another  example  of  Gaussian  mixture 
fitting  with  three  components.  Their  respective  weights 
are  0.40677,  0.39830,  and  0.19493.  In  this  case,  the  3a 
ellipses  are  rotated  with  respect  to  the  axes,  indicating 
large  correlation  between  the  two  variables  at  the  end  of 
simulation. 

It  is  reasonable  to  conclude  at  this  point  that  (1)  in  the 
cases  with  Gaussian  noise,  both  the  UKF/SPKF  and 
GMSPPF  outperform  the  EKF  and  (2)  the  UKF/SPFK 
performs  practically  as  good  as  the  GMSPPF  but  with 
significantly  less  computation.  It  is  therefore 
recommended  to  try  the  UKF/SPKF  first  and  then  to 
apply  the  GMSPPF  or  other  HKPF  if  the  computation 
power  permits. 

CONCLUSIONS 

In  this  paper,  we  outlined  three  classes  of  nonlinear 
filtering  techniques:  (1)  the  EKF  based  on  application  of 
linear  Gaussian  KF  to  linearized  process  and 
measurement  equations,  (2)  the  UKF  (SPKF)  based  on 
deterministic  discrete  approximation  of  Gaussian 
distributions  and  scaled  unscented  transformation  (SUT) 
through  nonlinear  functions,  and  (3)  the  PF  based  on 
random  samples  representation  of  distribution  and 
sequential  importance  sampling.  In  the  latter  case,  to 
avoid  sample  depletion  and  degeneracy,  the  resampling 
and  diversification  steps  are  needed.  To  bring  the 
transition  prior-based  proposal  density  close  to  the 
measurement  likelihood  function  for  importance  weight 
updating,  the  hybrid  Kalman  particle  filter  was  presented, 
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Figure  6  -  Orbit  Estimated  by  EKF 
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Figure  9  -  State  Estimation  Errors  for  KF 


Figure  10  -  Orbit  Estimated  by  UKF 


Figure  1 1  -  Monitor  1  ’s  Measurements  for  UKF 
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Figure  12  -  Monitor  2’s  Measurements  for  UKF  Figure  13  -  State  Estimation  Errors  for  UKF 


Figure  14  -  Orbit  Estimated  by  PF 
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Figure  15  -  Monitor  1  ’s  Measurements  for  PF  Figure  16  -  Monitor  2’s  Measurements  for  PF 


Figure  17  -  State  Estimation  Errors  for  PF 
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Figure  18  -  Orbit  Estimated  by  GMSPPF 


Figure  19  -  Monitor  1  ’s  Measurements  for 
GMSPPF 


Figure  20  -  Monitor  2’s  Measurements  for 
GMSPPF 
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Figure  21  -  State  Estimation  Errors  for  GMSPPF 
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Figure  22  -  Orbit  Arc  Visible  to  Two  Monitor 
Stations 
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Figure  23  -  State  Estimation  Errors  over  Long 
Orbit  Arc 
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Figure  24  -  GDOP  for  X-  and  Y-Components  Figure  25  -  Sigma-Points  Selected  for  Position, 

Velocity,  and  Acceleration 


Figure  26  -  Sigma  Points  Selected  for  Process 
Noise  and  Measurement  Noise 
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Figure  27  -  Initial  Particles  Drawn  from 
Estimated  Distribution 


Figure  28  -  Initial  Weights,  Predicted  Particles,  and 
Likelihood  for  X 


Figure  29  -  Updated  Weights,  State  Prior,  and 
Weights  After  Resampling 
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Figure  30  -  Weight  Index  Before  and  After 
Resampling 


Figure  3 1  -  Particle  Redrawing  with  Estimated 
Distribution 
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Figure  32  -  RMS  Values  for  Measurement 
Residuals 
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Figure  33  -  RMS  Values  for  Position  Errors  in  X 


Figure  34  -  RMS  Values  for  Position  Errors  in  Y 
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Figure  35  -  RMS  Values  for  Velocity  Errors  in  X 
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Figure  36  -  RMS  Values  for  Velocity  Errors  in  Y  Figure  37  -  Gaussian  Mixture  Fitting  to  Particles 


Figure  38  -  Gaussian  Mixture  Fitting  to  Particles 


in  which  an  EKP  or  UKF  (SPKF)  is  used  to  time- 
propagate  each  particle  or  each  component  of  a  Gaussian 
mixture  fitted  from  all  particles  and  measurement-update 
the  predicted  particle  or  Gaussian  mixture  component. 
The  resulting  posterior  state  distribution  is  used  as  the 
proposal  distribution  for  importance  sampling  (IS)-based 
measurement  updating  in  the  second  pass. 

This  paper  also  presented  computer  simulation  results  of 
four  nonlinear  filters  (EKF,  UKF/SPKF,  PF,  GMSPPF) 
applied  to  orbit  determination  using  the  kinematic 
method.  Better  results  are  expected  for  the  dynamic 
method  in  orbit  determination  (study  under  way)  where 
complicated  nonlinearities  are  involved  in  various  force 
models.  Of  particular  interest  in  our  current  study  is  the 
use  of  a  hybrid  Kalman  Particle  Filter  for  ultra-tightly 


coupled  GPS/IMU  [Abbott,  Lillo,  and  Douglas,  2000] 
using  the  grid-tracking  scheme  [Yang,  2003].  it  is 
expected  that  these  emerging  nonlinear  filtering 
techniques  will  gain  more  recognition  in  the  navigation 
community  and,  in  coupling  with  ever-increasing 
computing  power,  will  benefit  many  navigation 
applications. 
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